package models;

import odk.lang.FastMath;

public class MathHelper
{       
        public static double calcDistance(double x1, double x2, double y1, double y2, double z1, double z2)
        {
        	return FastMath.sqrt((x2 - x1) * (x2 - x1)
        					+ (y2 - y1) * (y2 - y1)
        					+ (z2 - z1) * (z2 - z1)) * 0.5f;
        }
        
        public static double calcDistanceWithoutSqrt(double x1, double x2, double y1, double y2, double z1, double z2)
        {
        	return ((x2 - x1) * (x2 - x1)
            		+ (y2 - y1) * (y2 - y1)
            		+ (z2 - z1) * (z2 - z1)) * 0.5f;
        }
        
        public static double sinf(double x)
        {
        	double sin;

        	//always wrap input angle to -PI..PI
        	if (x < -3.14159265)
        		x += 6.28318531;
        	else if (x >  3.14159265)
        		x -= 6.28318531;

        	//compute sine
        	if (x < 0)
        		sin = 1.27323954 * x + .405284735 * x * x;
        	else
        		sin = 1.27323954 * x - 0.405284735 * x * x;

        	return sin;
        }
        
        public static double cosf(double x)
        {
        	x = sinf(x);
        	
        	double cos;

        	//compute cosine: sin(x + PI/2) = cos(x)
        	x += 1.57079632;

        	if (x >  3.14159265)
        		x -= 6.28318531;

        	if (x < 0)
        		cos = 1.27323954 * x + 0.405284735 * x * x;
        	else
        		cos = 1.27323954 * x - 0.405284735 * x * x;

        	return cos;
        }
        
        public static double sin(double x)
        {
        	double sin;

        	//always wrap input angle to -PI..PI
        	if (x < -3.14159265)
        		x += 6.28318531;
        	else if (x >  3.14159265)
        		x -= 6.28318531;

        	if (x < 0)
        	{
        		sin = 1.27323954 * x + .405284735 * x * x;

        		if (sin < 0)
        			sin = .225 * (sin *-sin - sin) + sin;
        		else
        			sin = .225 * (sin * sin - sin) + sin;
        	}
        	else
        	{
        		sin = 1.27323954 * x - 0.405284735 * x * x;

        		if (sin < 0)
        			sin = .225 * (sin *-sin - sin) + sin;
        		else
        			sin = .225 * (sin * sin - sin) + sin;
        	}

        	return sin;
        }
        
        public static double cos(double x)
        {
        	x = sin(x);
        	
        	double cos;

        	//compute cosine: sin(x + PI/2) = cos(x)
        	x += 1.57079632;
        	if (x >  3.14159265)
        		x -= 6.28318531;

        	if (x < 0)
        	{
        		cos = 1.27323954 * x + 0.405284735 * x * x;

        		if (cos < 0)
        			cos = .225 * (cos *-cos - cos) + cos;
        		else
        			cos = .225 * (cos * cos - cos) + cos;
        	}
        	else
        	{
        		cos = 1.27323954 * x - 0.405284735 * x * x;

        		if (cos < 0)
        			cos = .225 * (cos *-cos - cos) + cos;
        		else
        			cos = .225 * (cos * cos - cos) + cos;
        	}

        	return cos;
        }
}